#include "componentMatch.h"
#include "segmentDifferences.h"


void shapeCutting( PointCloud<PointXYZRGB>::Ptr source,
                   PointCloud<PointXYZRGB>::Ptr targetComponent,
                   PointIndices::Ptr sourceComponentIndices,
                   double radius)
{
    KdTreeFLANN<PointXYZRGB> tree;
    tree.setInputCloud(source);
    vector<int> indices;
    vector<int> indicesInShape;
    vector<float> sqr_distances;

    VoxelGrid<PointXYZRGB> vox;
    vox.setInputCloud(targetComponent);
    vox.setLeafSize(radius,radius,radius);
    vox.filter(*targetComponent);

    for (int i=0;i<targetComponent->size();i++)
    {
        tree.radiusSearch(targetComponent->at(i),radius,indices,sqr_distances);
        for (int j=0;j<indices.size();j++) indicesInShape.push_back(indices.at(j));//inserisco tutte le sfere (con doppioni) in indicesInShape
    }
    sort(indicesInShape.begin(),indicesInShape.end());
    if(indicesInShape.size()==0)return;//non trova nessun punto
    sourceComponentIndices->indices.push_back(indicesInShape.back());
    while(!indicesInShape.empty())
    { //tolgo i doppioni
        if (sourceComponentIndices->indices.back()==indicesInShape.back())
            indicesInShape.pop_back();
        else
            sourceComponentIndices->indices.push_back(indicesInShape.back());
    }

}


void componentMatchClickSimulation
(
        PointCloud<PointXYZRGB>::Ptr registeredSource,
        PointCloud<PointXYZRGB>::Ptr targetCloud,
        Component componentToFind,//target
        PointIndices::Ptr sourceComponentIndices,
        vector<PointIndices> cluster_indices
        )
{
    PointCloud<PointXYZRGB>::Ptr registeredSourceCopy(new PointCloud<PointXYZRGB>);
    copyPointCloud(*registeredSource,*registeredSourceCopy);

    // Click simulation
    PointXYZRGB clickedPoint = targetCloud->at(componentToFind.getGeneratingIndex()); // extract the originally clicked point from the target cloud
    int sourceClickedPointIndex = registeredSourceCopy->size(); // the size of the cloud is the index of the next point that i add to it (che clicked point)
    registeredSourceCopy->push_back(clickedPoint); // push back the clicked point to use it for the segmentation

    // Component selection
    PointIndices::Ptr nonFilteredComponentIndices(new PointIndices);
    PointIndices::Ptr cluIndices(new PointIndices);

    //ATTRENZIONE I CLUSTYER ANDREBBERO TROVATI SULLA REGISTEREDCOPY PERCHE' HA IL PUNTO MAGICO
    //segmentCluster(cluIndices,cluster_indices, sourceClickedPointIndex );
    segmentClusterNearestPoint(registeredSource,cluIndices,cluster_indices, clickedPoint );
    PointIndices::Ptr colIndices(new PointIndices);
    segmentColor(registeredSourceCopy, colIndices, sourceClickedPointIndex, componentToFind.getColorThreshold());
    intersectIndices(cluIndices,colIndices,nonFilteredComponentIndices);

    //filtra con shape
    PointIndices::Ptr shapeIndices(new PointIndices);
    shapeCutting(registeredSource,componentToFind.getComponentCloud(),shapeIndices,5.0);
    reverse(nonFilteredComponentIndices->indices.begin(),nonFilteredComponentIndices->indices.end());
    intersectIndices(shapeIndices,nonFilteredComponentIndices,sourceComponentIndices);
}



void componentMatchShapeCutting( PointCloud<PointXYZRGB>::Ptr registeredSource,
                                 Component componentToFind,
                                 PointIndices::Ptr sourceComponentIndices)
{
    shapeCutting(registeredSource,componentToFind.getComponentCloud(),sourceComponentIndices,componentToFind.getSkinThickness());
}













